/***************************************************************************
 创建者: 华磊
 开始时间: 2018.12.13
 copyright: (C) 华友高科
 ***************************************************************************/
#include "laserdriversicklms.h"
#include <LMS1xx/LMS1xx.h>

#define DEG2RAD M_PI/180.0

LaserDriverSickLms::LaserDriverSickLms(LaserDriverConfig configIn)
                                      :LaserDriverAbstract(configIn)
{
    isShutDown=false;
    scan_pub=new ros::Publisher;
    initialDriver(configIn);
    createThread();
}

int LaserDriverSickLms::initialDriver(LaserDriverConfig configIn)
{
    // laser data
    laser=new LMS1xx;
    scanCfg cfg;
    scanOutputRange outputRange;
    scanDataCfg dataCfg;


    // parameters
    std::string host=configIn.hostname;;
    std::string frame_id=configIn.frame_id;;

    int port=atoi(configIn.port.c_str());

//    ros::init(argc, argv, "lms1xx");
    ros::NodeHandle nh;
//    ros::NodeHandle n("~");
    *scan_pub = nh.advertise<sensor_msgs::LaserScan>(configIn.topicName, 1);

//    n.param<std::string>("host", host, "192.168.1.2");
//    n.param<std::string>("frame_id", frame_id, "laser");
//    n.param<bool>("publish_min_range_as_inf", inf_range, false);
//    n.param<int>("port", port, 2111);

    int tryCount=0;
    while(tryCount<7)
    {
      tryCount++;
      ROS_INFO_STREAM("Connecting to laser at " << host);
      laser->connect(host, port);
      if (!laser->isConnected())
      {
        ROS_WARN("Unable to connect, retrying.");
        ros::Duration(1).sleep();
        continue;
      }

      ROS_DEBUG("Logging in to laser->");
      laser->login();
      cfg = laser->getScanCfg();
      outputRange = laser->getScanOutputRange();

      if (cfg.scaningFrequency != 5000)
      {
        laser->disconnect();
        ROS_WARN("Unable to get laser output range. Retrying.");
        ros::Duration(1).sleep();
        continue;
      }

      ROS_INFO("Connected to laser->");

      ROS_DEBUG("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d",
                cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, cfg.stopAngle);
      ROS_DEBUG("Laser output range:angleResolution %d, startAngle %d, stopAngle %d",
                outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle);

      scan_msg.header.frame_id = frame_id;
      scan_msg.range_min = 0.01;
      scan_msg.range_max = 20.0;
      scan_msg.scan_time = 100.0 / cfg.scaningFrequency;
      scan_msg.angle_increment = static_cast<double>(outputRange.angleResolution / 10000.0 * DEG2RAD);
      scan_msg.angle_min = static_cast<double>(outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
      scan_msg.angle_max = static_cast<double>(outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);

      ROS_DEBUG_STREAM("Device resolution is " << (double)outputRange.angleResolution / 10000.0 << " degrees.");
      ROS_DEBUG_STREAM("Device frequency is " << (double)cfg.scaningFrequency / 100.0 << " Hz");

      int angle_range = outputRange.stopAngle - outputRange.startAngle;
      int num_values = angle_range / outputRange.angleResolution;
      if (angle_range % outputRange.angleResolution == 0)
      {
        // Include endpoint
        ++num_values;
      }
      scan_msg.ranges.resize(num_values);
      scan_msg.intensities.resize(num_values);

      scan_msg.time_increment =
        (outputRange.angleResolution / 10000.0)
        / 360.0
        / (cfg.scaningFrequency / 100.0);

      ROS_DEBUG_STREAM("Time increment is " << static_cast<int>(scan_msg.time_increment * 1000000) << " microseconds");

      dataCfg.outputChannel = 1;
      dataCfg.remission = true;
      dataCfg.resolution = 1;
      dataCfg.encoder = 0;
      dataCfg.position = false;
      dataCfg.deviceName = false;
      dataCfg.outputInterval = 1;

      ROS_DEBUG("Setting scan data configuration.");
      laser->setScanDataCfg(dataCfg);

      ROS_DEBUG("Starting measurements.");
      laser->startMeas();

      ROS_DEBUG("Waiting for ready status.");
      ros::Time ready_status_timeout = ros::Time::now() + ros::Duration(5);

      // while(1)
      // {
      status_t stat = laser->queryStatus();
      ros::Duration(1.0).sleep();
      if (stat != ready_for_measurement)
      {
        ROS_WARN("Laser not ready. Retrying initialization.");
        laser->disconnect();
        ros::Duration(1).sleep();
        continue;
      }
      /*if (stat == ready_for_measurement)
      {
        ROS_DEBUG("Ready status achieved.");
        break;
      }

        if (ros::Time::now() > ready_status_timeout)
        {
          ROS_WARN("Timed out waiting for ready status. Trying again.");
          laser->disconnect();
          continue;
        }

        if (!ros::ok())
        {
          laser->disconnect();
          return 1;
        }
      }*/

      ROS_DEBUG("Starting device.");
      laser->startDevice();  // Log out to properly re-enable system after config

      ROS_DEBUG("Commanding continuous measurements.");
      laser->scanContinous(1);
    }

    if(7<=tryCount)
    {
        return -1;
    }
    return 1;

}

int LaserDriverSickLms::publishLaserDataLoopOnce()
{
//    int result = sick_tim::ExitError;
//    if(NULL!=sickTimDriver)
//    {
////        sickTimDriver->loopOnce();//加多一次，可能反应慢.不能加多，会把customtime 拖到１３６ｍｓ
//        result =sickTimDriver->loopOnce();
//    }
//    else
//    {

//    }

//    if(sick_tim::ExitSuccess==result)
//    {
//        return 1;
//    }
//    return -1;
    return 1;
}

int LaserDriverSickLms::shutDownDriver()
{
    qDebug()<<"LaserDriverSickLms shutDownDriver";
    isShutDown=true;
    if(NULL!=laser)
    {
        laser->scanContinous(0);
        laser->stopMeas();
        laser->disconnect();
        delete laser;
    }
    return 1;
}

bool LaserDriverSickLms::isDriverStatusOk()
{
    return isDriverOk;
}

int LaserDriverSickLms::loopOnce()
{
    bool inf_range=false;
    ros::Time start = ros::Time::now();

    scan_msg.header.stamp = start;
    ++scan_msg.header.seq;

    scanData data;
    ROS_DEBUG("Reading scan data.");
    if (laser->getScanData(&data))
    {
      for (int i = 0; i < data.dist_len1; i++)
      {
        float range_data = data.dist1[i] * 0.001;

        if (inf_range && range_data < scan_msg.range_min)
        {
          scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
        }
        else
        {
          scan_msg.ranges[i] = range_data;
        }
      }

      for (int i = 0; i < data.rssi_len1; i++)
      {
        scan_msg.intensities[i] = data.rssi1[i];
      }

      ROS_DEBUG("Publishing scan data.");
      scan_pub->publish(scan_msg);
    }
    else
    {
      ROS_ERROR("Laser timed out on delivering scan, attempting to reinitialize.");
      return -1;
    }

//    ros::spinOnce();
    return 1;
}

void* LaserDriverSickLms::threadRun(void *)
{
    printf("LaserDriverSickLms theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 30;
    sched_setscheduler(0,SCHED_FIFO,&param);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    while(false==isShutDown)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
//         QThread::msleep(1000);
            if(NULL!=laser)
            {
        //        sickTimDriver->loopOnce();//加多一次，可能反应慢.不能加多，会把customtime 拖到１３６ｍｓ
                loopOnce();
            }

    }
    printf("LaserDriverSickLms thread quit!!!");
}

void* LaserDriverSickLms::threadTask( void* classPtr )
{
    qDebug()<<"LaserDriverSickLms thread run-------------------------";
    return ((LaserDriverSickLms*)classPtr)->threadRun(NULL);
}

void LaserDriverSickLms::createThread()
{
    int ret=pthread_create( &threadId, NULL, &LaserDriverSickLms::threadTask,this);
    qDebug()<<"LaserDriverSickLms createThread-------------------------";
}

